from i2cport.mp_pca9685 import MpPca9685
from uwb.tag import Tag
import time
import math

class UWBCarController:
    def __init__(self, com_port='COM5', base_distance=1.8):
        # 初始化硬件组件
        self.tag = Tag(com_port, base_distance=base_distance)
        self.pca9685 = MpPca9685(0x40, 50)

        # 控制参数
        self.ANGLE_THRESHOLD = 15
        self.BASE_SPEED = 100
        # self.TURN_GAIN = 1.5
        self.LEFT_PIN = 0
        self.RIGHT_PIN = 1
        # self.MIN_MOVE_DISTANCE = 0.2
        # self.DIRECTION_UPDATE_INTERVAL = 1

        # # 状态变量
        # self.last_x = None
        # self.last_y = None
        # self.last_time = None
        # self.current_angle = 0

        # 新增转向控制参数
        self.TURN_BASE_SPEED = 150  # 转向基准速度
        self.DEGREE_PER_SECOND = 90  # 每秒可转角度数（需实际校准）
        self.last_turn_time = 0

        # 新增校准参数
        self.CALIBRATION_DURATION = 1.0  # 默认校准持续时间（秒）
        self.MIN_CALIBRATION_DISTANCE = 0.2  # 最小有效校准距离
        self.SPEED_OFFSET = 8  # 根据实际偏差调整这个值（正值表示右轮加速/左轮减速）

    def _get_current_coordinates(self):
        """通用坐标获取方法"""
        distance_list = self.tag.get_distance()
        return self.tag.calculate_tag_coordinates(distance_list)

    def _calculate_direction(self, start_x, start_y, end_x, end_y):
        """通用方向计算逻辑"""
        dx = end_x - start_x
        dy = end_y - start_y
        return math.degrees(math.atan2(dy, dx))

    def _execute_timed_straight(self, duration, stop_condition=None):
        """通用定时直行方法"""
        start_time = time.perf_counter()
        while time.perf_counter() - start_time < duration:
            self._fixed_straight()
            if stop_condition and stop_condition():
                return True
            time.sleep(0.05)
        return False

    def calibrate_direction(self, duration=3):
        """优化后的校准方法"""
        # 获取初始坐标
        start_x, start_y = self._get_current_coordinates()

        # 执行校准移动
        self._execute_timed_straight(duration)

        # 获取结束坐标并验证
        end_x, end_y = self._get_current_coordinates()
        if math.hypot(end_x - start_x, end_y - start_y) < self.MIN_CALIBRATION_DISTANCE:
            raise ValueError("校准移动距离不足，请确保地面标记清晰")

        return self._calculate_direction(start_x, start_y, end_x, end_y)

    def move_to_target(self, x1, y1):
        """优化后的移动方法"""
        self.current_angle = self.calibrate_direction(5)

        while True:
            x, y = self._get_current_coordinates()

            # 到达判断
            if math.hypot(x1 - x, y1 - y) < 0.5:
                self.stop()
                break

            # 计算目标方向偏差
            target_angle = self._calculate_direction(x, y, x1, y1)
            angle_error = (target_angle - self.current_angle) % 360
            angle_error = angle_error - 360 if angle_error > 180 else angle_error

            # 转向修正
            if abs(angle_error) > self.ANGLE_THRESHOLD:
                self._drive_motors(angle_error)

            # 执行带实时检测的直行
            def should_stop():
                current_x, current_y = self._get_current_coordinates()
                return math.hypot(x1 - current_x, y1 - current_y) < 0.5

            if self._execute_timed_straight(1, should_stop):
                self.stop()
                return

            # 更新当前角度
            current_x, current_y = self._get_current_coordinates()
            self.current_angle = self._calculate_direction(x, y, current_x, current_y)

    def _update_direction_info(self, current_x, current_y):

        """集成时间判断和坐标更新的完整流程"""
        """集成时间判断和坐标更新的完整流程"""
        global last_x, last_y, last_time

        if time.time() - last_time > self.DIRECTION_UPDATE_INTERVAL:
            new_angle, new_time = self._calculate_direction_by_time(current_x, current_y)
            # 更新全局坐标和时间
            last_x, last_y = current_x, current_y
            last_time = time.time()
            self.current_angle = new_angle


    def _calculate_direction_by_time(self, current_x, current_y):

        """根据时间间隔计算方向角度"""
        time_diff = time.perf_counter() - last_time
        if time_diff < 0.1 or last_x is None or last_y is None:
            return self.current_angle, last_time  # 保持原角度

        dx = current_x - last_x
        dy = current_y - last_y
        move_distance = math.hypot(dx, dy)

        if move_distance > self.MIN_MOVE_DISTANCE:
            new_angle = math.degrees(math.atan2(dy, dx))
            # 使用加权平均平滑角度变化
            return (self.current_angle * 0.3 + new_angle * 0.7), time.time()
        return self.current_angle, last_time

    def _drive_motors(self, angle_error):
        """简化版驱动控制，使用定时转向"""
        if abs(angle_error) > self.ANGLE_THRESHOLD:
            # 计算需要转向的时间（角度/每秒可转角度）
            # 添加最小转向时间限制（防止过短转向）
            turn_time = abs(angle_error) / self.DEGREE_PER_SECOND
            turn_time = max(turn_time, 0.3)  # 最少转向0.3秒
            direction = 'left' if angle_error < 0 else 'right'

            # 执行转向
            self._fixed_turn(direction, turn_time)
        else:
            # 直行模式
            self._fixed_straight()

    def _fixed_straight(self):
        left_speed = self.BASE_SPEED + self.SPEED_OFFSET
        right_speed = self.BASE_SPEED - self.SPEED_OFFSET
        """固定速度直行"""
        self.pca9685.command_drive_servo_car(
            left_pin=self.LEFT_PIN,
            left_speed=left_speed,
            right_pin=self.RIGHT_PIN,
            right_speed=right_speed
        )

    def _fixed_turn(self, direction, duration):
        """固定速度转向"""
        start_time = time.perf_counter()
        while time.perf_counter() - start_time < duration:
            if direction == 'left':
                self.pca9685.command_drive_servo_car(
                    self.LEFT_PIN, -self.TURN_BASE_SPEED,
                    self.RIGHT_PIN, self.TURN_BASE_SPEED
                )
            else:
                self.pca9685.command_drive_servo_car(
                    self.LEFT_PIN, self.TURN_BASE_SPEED,
                    self.RIGHT_PIN, -self.TURN_BASE_SPEED
                )
            time.sleep(0.01)
        self.stop()

    def stop(self):
        """立即停止小车"""
        self.pca9685.command_drive_servo_car(
            self.LEFT_PIN, 0,
            self.RIGHT_PIN, 0
        )



# 使用示例
if __name__ == '__main__':
    car = UWBCarController()
    car.stop()
    time.sleep(2)